#ifndef LIDAR_LOCALIZATION_MODELS_GTSAM_OPTIMIZER_HPP_
#define LIDAR_LOCALIZATION_MODELS_GTSAM_OPTIMIZER_HPP_
#include <iostream>
#include <fstream>
#include <Eigen/Dense>
#include <gtsam/slam/PriorFactor.h> //设置相当于fixed顶点
#include <gtsam/slam/BetweenFactor.h>  //设置相当于边

#include <gtsam/nonlinear/LevenbergMarquardtParams.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/navigation/GPSFactor.h>

#include <vector>
#include <string>

namespace lidar_localization
{
using namespace std;
using namespace gtsam;
typedef Eigen::Matrix<double,6,1> Vector6d;
struct cereNode
{
    /* data */
    unsigned int index;
    double t[3];
    double q[4];
    bool need_fix = false;
};
struct cereEdge
{
    unsigned int index1;
    unsigned int index2;
    double t[3] ;
    double q[4];
    Vector6d conversie;
};

struct cereEdgeXYZ
{
    unsigned int index;
    double t[3];
    Eigen::Vector3d conversie;
};
class GtsamGraphOptimizer {
public:
    GtsamGraphOptimizer();
    // 优化
    bool Optimize() ;
    // 输入、输出数据
    bool GetOptimizedPose(std::deque<Eigen::Matrix4f>& optimized_pose) ;
    int GetNodeNum() ;
    // 添加节点、边、鲁棒核
    // void SetEdgeRobustKernel(std::string robust_kernel_name, double robust_kernel_size){}
    void AddSe3Node(const Eigen::Isometry3d &pose, bool need_fix) ;
    void AddSe3Edge(int vertex_index1,
                            int vertex_index2,
                            const Eigen::Isometry3d &relative_pose,
                            Vector6d noise);
    void AddSe3PriorXYZEdge(int se3_vertex_index,
                                    const Eigen::Vector3d &xyz,
                                    Eigen::Vector3d noise);
    void AddSe3PriorQuaternionEdge(int se3_vertex_index,
                                           const Eigen::Quaterniond &quat,
                                           Eigen::VectorXd noise);
    // 设置优化参数
    void SetMaxIterationsNum(int max_iterations_num);
private:
    std::vector<cereNode> nodeDataBuff;
    std::vector<cereEdge> edgeDataBuff;
    std::vector<cereEdgeXYZ> edgeXYZDatabuff;



    noiseModel::Diagonal::shared_ptr edgeNoise;
    noiseModel::Diagonal::shared_ptr edgexyzNoise;
    unsigned int num = 0 ;



    NonlinearFactorGraph graph;    //图模型
    Values initial;                //顶点
    
    std::unique_ptr<ISAM2> isam;
    Values isamCurrentEstimate;


};







}


#endif